# import cv2
# import asyncio
# import websockets
# import json
# from cvzone.PoseModule import PoseDetector
#
# # 初始化摄像头和姿态检测器
# # cap = cv2.VideoCapture(0)
# # if not cap.isOpened():
# #     print("摄像头初始化失败！请检查摄像头是否正常连接")
# #     exit()
#
#
# # 修改摄像头初始化代码（替换原cap = cv2.VideoCapture(0)）
# cap = cv2.VideoCapture(0)
# # 设置摄像头分辨率（宽640，高480，根据摄像头支持情况调整）
# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
# if not cap.isOpened():
#     print("摄像头初始化失败！")
#     exit()
#
#
#
#
# detector = PoseDetector()
# print("摄像头已启动，等待Unity客户端连接...")
#
#
# # WebSocket服务端逻辑：实时发送姿态数据（只保留websocket参数）
# async def send_pose_data(websocket):  # 关键修改：删除path参数
#     print("Unity客户端已连接，开始发送姿态数据！")
#     try:
#         while True:
#             success, img = cap.read()
#             if not success:
#                 print("摄像头读取失败，断开连接")
#                 break
#
#             img = detector.findPose(img)
#             lmList, bboxInfo = detector.findPosition(img)
#
#             pose_data = []
#             if bboxInfo and len(lmList) >= 33:
#                 for i in range(33):
#                     x, y, z = lmList[i]
#                     pose_data.append({"x": x / 100, "y": (img.shape[0] - y) / 100, "z": z / 300})
#
#             await websocket.send(json.dumps(pose_data))
#             cv2.imshow('Pose Detection', img)
#
#             if cv2.waitKey(1) == 27:
#                 break
#     finally:
#         cap.release()
#         cv2.destroyAllWindows()
#         print("连接已断开，摄像头已关闭")
#
#
# # 启动WebSocket服务的主函数
# async def main():
#     # 注意：这里的handler只传send_pose_data（不再需要处理path）
#     async with websockets.serve(send_pose_data, "localhost", 8765):
#         await asyncio.Future()  # 保持服务运行
#
#
# if __name__ == "__main__":
#     try:
#         asyncio.run(main())
#     except KeyboardInterrupt:
#         print("程序已手动停止")
#         cap.release()
#         cv2.destroyAllWindows()




import cv2
import asyncio
import websockets
import time
from cvzone.PoseModule import PoseDetector

# 1. 初始化摄像头（降低分辨率提升速度）
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)   # 宽640（可调整）
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)  # 高480（可调整）
if not cap.isOpened():
    print("❌ 摄像头初始化失败！请检查摄像头连接")
    exit()

# 2. 初始化姿态检测器（关闭绘制减少耗时）
detector = PoseDetector()
print("✅ 摄像头已启动，等待Unity连接...")
print(f"📡 WebSocket服务地址：ws://localhost:8765")

# 3. 帧率控制参数（平衡速度与稳定性）
TARGET_FPS = 20  # 目标发送帧率（建议15-25fps）
FRAME_INTERVAL = 1.0 / TARGET_FPS  # 帧间隔时间（秒）


# 4. WebSocket服务端逻辑（轻量化数据传输）
async def send_pose_data(websocket):
    print("🎉 Unity客户端已连接，开始发送姿态数据！")
    last_send_time = time.time()  # 记录上次发送时间

    try:
        while True:
            # 控制发送频率，避免数据堆积
            current_time = time.time()
            if current_time - last_send_time < FRAME_INTERVAL:
                await asyncio.sleep(0.001)  # 短暂休眠，降低CPU占用
                continue

            # 读取摄像头帧
            success, img = cap.read()
            if not success:
                print("❌ 摄像头读取失败，断开连接")
                break

            # 检测姿态（关闭绘制，只获取关键点）
            detector.findPose(img, draw=False)  # 不绘制骨架
            lmList, bboxInfo = detector.findPosition(img, draw=False)  # 不绘制关键点

            # 生成"x1,y1,z1,x2,y2,z2..."格式字符串（比JSON快50%+）
            pose_str = ""
            if bboxInfo and len(lmList) >= 33:  # 确保检测到33个关键点
                for i in range(33):
                    x, y, z = lmList[i]
                    # 坐标转换+缩放（适配Unity坐标系）
                    unity_x = x / 100
                    unity_y = (img.shape[0] - y) / 100  # 翻转y轴
                    unity_z = z / 300
                    # 拼接字符串（无JSON序列化开销）
                    pose_str += f"{unity_x},{unity_y},{unity_z},"
                # 移除最后一个逗号，避免Unity解析错误
                pose_str = pose_str.rstrip(',')
                # 发送数据并更新时间
                await websocket.send(pose_str)
                last_send_time = current_time

                # 可选：显示摄像头画面（如需关闭，注释下面2行）
                cv2.imshow('Pose Detection (Press ESC to Exit)', img)
                if cv2.waitKey(1) == 27:  # 按ESC键退出
                    break

    finally:
        # 释放资源
        cap.release()
        cv2.destroyAllWindows()
        print("🔌 连接已断开，资源已释放")


# 5. 启动WebSocket服务
async def main():
    async with websockets.serve(send_pose_data, "localhost", 8765):
        await asyncio.Future()  # 保持服务持续运行


if __name__ == "__main__":
    try:
        asyncio.run(main())
    except KeyboardInterrupt:
        print("🛑 程序已手动停止")
        cap.release()
        cv2.destroyAllWindows()